#include "distributedMapping.h"

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
	class distributedMapping: handle message callback
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
void distributedMapping::globalDescriptorHandler(
	const dcl_slam::global_descriptorConstPtr &msg,
	int &id)
{
	// save timestamp
	robots[id].time_cloud_input_stamp = msg->header.stamp;
	robots[id].time_cloud_input = robots[id].time_cloud_input_stamp.toSec();

	// save descriptors
	LOG(INFO) << "[globalDescriptorHandler(" << id << ")]"
			  << " saveDescriptorAndKey:" << msg->index << "." << endl;
	// keyframe_descriptor->saveDescriptorAndKey(msg->values.data(), id, msg->index);
	store_descriptors.emplace_back(make_pair(id, *msg));
}

void distributedMapping::optStateHandler(
	const std_msgs::Int8ConstPtr &msg,
	int &id)
{
	neighbors_started_optimization[id] = (OptimizerState)msg->data <= OptimizerState::Start;
	neighbor_state[id] = (OptimizerState)msg->data;
	neighbors_lowest_id_included[id] = lowest_id_included;
	if (neighbors_within_communication_range.find(id) == neighbors_within_communication_range.end())
	{
		neighbors_within_communication_range.insert(id);
	}
}

void distributedMapping::rotationStateHandler(
	const std_msgs::Int8ConstPtr &msg,
	int &id)
{
	neighbors_rotation_estimate_finished[id] = msg->data;
}

void distributedMapping::poseStateHandler(
	const std_msgs::Int8ConstPtr &msg,
	int &id)
{
	neighbors_pose_estimate_finished[id] = msg->data;
	neighbors_estimation_done[id] = msg->data;
}
/* 函数接收两个参数：
msg: 指向邻居机器人发送的 neighbor_estimate 消息的指针
id: 邻居机器人的 ID */
void distributedMapping::neighborRotationHandler(
	const dcl_slam::neighbor_estimateConstPtr &msg,
	int &id)
{
	/* 检查消息是否是发送给本机器人的，并且本机器人处于旋转估计状态。如果不满足条件，则退出函数。
 */
	if (msg->receiver_id != id_ || optimizer_state != OptimizerState::RotationEstimation)
	{
		return;
	}

	/*** prepare for optimization ***/
	// update neighbor roatation estimates
	// 更新邻居机器人的旋转估计值和标志位，为优化做准备。
	for (int i = 0; i < msg->pose_id.size(); i++)
	{
		/* 如果所有其他邻居机器人都完成了旋转优化，则进行以下操作：
		执行旋转估计
		检查优化是否完成
		将旋转估计发送给后续机器人
		发送旋转优化状态 */
		Symbol symbol((id + 'a'), msg->pose_id[i]);
		if (!use_pcm_ || other_robot_keys_for_optimization.find(symbol.key()) != other_robot_keys_for_optimization.end())
		{
			Vector rotation_matrix(9);
			rotation_matrix << msg->estimate[0 + 9 * i], msg->estimate[1 + 9 * i], msg->estimate[2 + 9 * i],
				msg->estimate[3 + 9 * i], msg->estimate[4 + 9 * i], msg->estimate[5 + 9 * i],
				msg->estimate[6 + 9 * i], msg->estimate[7 + 9 * i], msg->estimate[8 + 9 * i];
			optimizer->updateNeighborLinearizedRotations(symbol.key(), rotation_matrix);
		}
		else
		{
			ROS_DEBUG("Stop optimization<%d>. Key %d %d doesn't exist.", id_, symbol.chr(), symbol.index());
			abortOptimization(false);
		}
	}
	// update neighbor flags
	if (optimizer_state == OptimizerState::RotationEstimation)
	{
		// used only with flagged initialization
		optimizer->updateNeighboringRobotInitialized(char(id + 'a'), msg->initialized);
		neighbors_rotation_estimate_finished[id] = msg->estimation_done;
	}
	ROS_DEBUG("neighborRotationHandler<%d> from robot %d done? %d (%d/%d]", id_, id, msg->estimation_done,
			  optimizer->getNeighboringRobotsInit().size(), optimization_order.size() - 1);

	/*** perform rotation optimization ***/
	// all other robot is finished rotation optimization
	if (optimizer->getNeighboringRobotsInit().size() == optimization_order.size() - 1)
	{
		if (!estimation_done)
		{
			// rotation estimation
			try
			{
				optimizer->estimateRotation();
				optimizer->updateRotation();
				optimizer->updateInitialized(true);
				current_rotation_estimate_iteration++;
			}
			catch (const std::exception &ex)
			{
				ROS_WARN("Stopping rotation optimization<%d> : %s.", id_, ex.what());
				abortOptimization(true);
			}

			// if change is small enough, end rotation optimization
			if ((optimizer->latestChange() <= rotation_estimate_change_threshold_) ||
				(current_rotation_estimate_iteration >= optmization_maximum_iteration_))
			{
				rotation_estimate_finished = true;
				estimation_done = true;
			}
			ROS_DEBUG("--->Rotation estimation<%d> iter:[%d/%d] change:%.4f.", id_,
					  current_rotation_estimate_iteration, optmization_maximum_iteration_, optimizer->latestChange());
		}

		// check neigbors rotation optimization state
		bool send_flag = estimation_done;
		for (int i = 0; i < optimization_order.size(); i++)
		{
			int other_robot = optimization_order[i];
			if (!neighbors_rotation_estimate_finished[other_robot] && other_robot != id_)
			{
				send_flag = false;
			}
		}
		// send rotation estimate to the aft-order robot
		if (!send_flag)
		{
			// clear buffer
			for (const auto &neighbor : neighbors_within_communication_range)
			{
				robots[neighbor].estimate_msg.pose_id.clear();
				robots[neighbor].estimate_msg.estimate.clear();
			}
			// extract rotation estimate for each loop closure
			for (const std::pair<Symbol, Symbol> &separator_symbols : optimizer->separatorsSymbols())
			{
				// robot id
				int other_robot = (int)(separator_symbols.first.chr() - 'a');
				// pose id
				robots[other_robot].estimate_msg.pose_id.push_back(separator_symbols.second.index());
				// rotation estimates
				Vector rotation_estimate = optimizer->linearizedRotationAt(separator_symbols.second.key());
				for (int it = 0; it < 9; it++)
				{
					robots[other_robot].estimate_msg.estimate.push_back(rotation_estimate[it]);
				}
			}
			// send rotation estimate
			int publish_flag = false;
			for (int i = 0; i < optimization_order.size(); i++)
			{
				int other_robot = optimization_order[i];
				if (other_robot == id_)
				{
					publish_flag = true;
					continue;
				}

				if (publish_flag)
				{
					robots[other_robot].estimate_msg.initialized = optimizer->isRobotInitialized();
					robots[other_robot].estimate_msg.receiver_id = other_robot;
					robots[other_robot].estimate_msg.estimation_done = estimation_done;
					robots[id_].pub_neighbor_rotation_estimates.publish(robots[other_robot].estimate_msg);
				}
			}

			rotation_estimate_start = false;
			optimizer->clearNeighboringRobotInit();
		}
		// send rotation optimization state
		state_msg.data = estimation_done ? 1 : 0;
		robots[id_].pub_rotation_estimate_state.publish(state_msg);
	}
}

void distributedMapping::neighborPoseHandler(
	const dcl_slam::neighbor_estimateConstPtr &msg,
	int &id)
{
	if (msg->receiver_id != id_ || optimizer_state != OptimizerState::PoseEstimation)
	{
		return;
	}

	/*** prepare for optimization ***/
	// save neighbor anchor offset
	if (msg->estimation_done)
	{
		Point3 offset(msg->anchor_offset[0], msg->anchor_offset[1], msg->anchor_offset[2]);
		neighbors_anchor_offset[id] = offset;
	}
	// update neighbor pose estimates
	for (int i = 0; i < msg->pose_id.size(); i++)
	{
		if (neighbors_within_communication_range.find(id) != neighbors_within_communication_range.end())
		{
			Symbol symbol((id + 'a'), msg->pose_id[i]);
			Vector pose_vector(6);
			pose_vector << msg->estimate[0 + 6 * i], msg->estimate[1 + 6 * i], msg->estimate[2 + 6 * i],
				msg->estimate[3 + 6 * i], msg->estimate[4 + 6 * i], msg->estimate[5 + 6 * i];
			optimizer->updateNeighborLinearizedPoses(symbol.key(), pose_vector);
		}
	}
	// update neighbor flags
	if (optimizer_state == OptimizerState::PoseEstimation)
	{
		// used only with flagged initialization
		optimizer->updateNeighboringRobotInitialized(char(id + 'a'), msg->initialized);
		neighbors_estimation_done[id] = msg->estimation_done;
		neighbors_pose_estimate_finished[id] = msg->estimation_done;
	}
	ROS_DEBUG("neighborPoseHandler<%d> from robot %d, done? %d [%d/%d]", id_, id, msg->estimation_done,
			  optimizer->getNeighboringRobotsInit().size(), optimization_order.size() - 1);

	/*** perform pose estimation ***/
	if (optimizer->getNeighboringRobotsInit().size() == optimization_order.size() - 1)
	{
		if (!estimation_done)
		{
			// pose Estimation
			try
			{
				optimizer->estimatePoses();
				optimizer->updatePoses();
				optimizer->updateInitialized(true);
				current_pose_estimate_iteration++;
			}
			catch (const std::exception &ex)
			{
				ROS_WARN("Stopping pose optimization<%d> : %s.", id_, ex.what());
				abortOptimization(true);
			}

			// if change is small enough, end pose optimization
			if ((current_pose_estimate_iteration >= optmization_maximum_iteration_) ||
				(optimizer->latestChange() <= pose_estimate_change_threshold_))
			{
				pose_estimate_finished = true;
				estimation_done = true;
			}
			ROS_DEBUG("--->Pose estimation<%d> iter:[%d/%d] change:%.4f.", id_,
					  current_pose_estimate_iteration, optmization_maximum_iteration_, optimizer->latestChange());

			// extract anchor offset
			Key first_key = KeyVector(initial_values->keys()).at(0);
			anchor_point = initial_values->at<Pose3>(first_key).translation();
			anchor_offset = anchor_point - (optimizer->currentEstimate().at<Pose3>(first_key).translation() +
											Point3(optimizer->linearizedPoses().at(first_key).tail(3)));
		}

		// check neigbors rotation optimization state
		bool send_flag = estimation_done;
		for (int i = 0; i < optimization_order.size(); i++)
		{
			int other_robot = optimization_order[i];
			if (!neighbors_pose_estimate_finished[other_robot] && other_robot != id_)
			{
				send_flag = false;
			}
		}
		// send pose estimate to the aft-order robot
		if (!send_flag)
		{
			// clear buffer
			for (const auto &neighbor : neighbors_within_communication_range)
			{
				robots[neighbor].estimate_msg.pose_id.clear();
				robots[neighbor].estimate_msg.estimate.clear();
				robots[neighbor].estimate_msg.anchor_offset.clear();
			}
			// extract pose estimate from each loop closure
			for (const std::pair<Symbol, Symbol> &separator_symbols : optimizer->separatorsSymbols())
			{
				int other_robot = (int)(separator_symbols.first.chr() - 'a');

				robots[other_robot].estimate_msg.pose_id.push_back(separator_symbols.second.index());

				Vector pose_estimate = optimizer->linearizedPosesAt(separator_symbols.second.key());
				for (int it = 0; it < 6; it++)
				{
					robots[other_robot].estimate_msg.estimate.push_back(pose_estimate[it]);
				}
			}
			// send pose estimate
			bool publish_flag = false;
			for (int i = 0; i < optimization_order.size(); i++)
			{
				int other_robot = optimization_order[i];
				if (other_robot == id_)
				{
					publish_flag = true;
					continue;
				}

				if (publish_flag)
				{
					for (int i = 0; i < 3; i++)
					{
						robots[other_robot].estimate_msg.anchor_offset.push_back(anchor_offset.vector()[i]);
					}
					robots[other_robot].estimate_msg.initialized = optimizer->isRobotInitialized();
					robots[other_robot].estimate_msg.receiver_id = other_robot;
					robots[other_robot].estimate_msg.estimation_done = estimation_done;
					robots[id_].pub_neighbor_pose_estimates.publish(robots[other_robot].estimate_msg);
				}
			}
			// send pose optimization state
			pose_estimate_start = false;
			optimizer->clearNeighboringRobotInit();
		}
		// send optimization state
		state_msg.data = estimation_done ? 1 : 0;
		robots[id_].pub_pose_estimate_state.publish(state_msg);
	}
}

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
	class distributedMapping: saving keyframe API
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
// 预先准备分布式地图，并执行分布式一致性最大化算法。保存关键帧API
void distributedMapping::performDistributedMapping(
	const Pose3 &pose_to,
	const pcl::PointCloud<PointPose3D>::Ptr frame_to,
	const ros::Time &timestamp)
{
	// save keyframe cloud
	// 将frame_to点云点的3D坐标保存到robots[id_].keyframe_cloud中
	pcl::copyPointCloud(*frame_to, *robots[id_].keyframe_cloud);
	// 向robots[id_].keyframe_cloud_array容器中添加点云
	robots[id_].keyframe_cloud_array.push_back(*robots[id_].keyframe_cloud);
	// save timestamp
	robots[id_].time_cloud_input_stamp = timestamp;
	robots[id_].time_cloud_input = timestamp.toSec();

	// add piror factor添加先验因素
	Pose3 new_pose_to;

	int poses_num = initial_values->size();
	Symbol current_symbol = Symbol('a' + id_, poses_num);
	if (poses_num == 0) // initial_values是否为空。如果为空，添加先验因素
	{
		// save piror value
		// 将变量pose_to的值保存到robots[id_].piror_odom中。这里假设robots是一个包含多个机器人的数组，id_表示当前机器人的索引。
		robots[id_].piror_odom = pose_to;

		// add piror factor to graph
		// current_symbol 是这个先验因子所关联的变量的标识符或索引，pose_to 是该先验所对应的目标值，prior_noise 则表示先验的噪声模型
		auto prior_factor = PriorFactor<Pose3>(current_symbol, pose_to, prior_noise);
		// 将先验输入到一个没有pcm的局部位姿因子图中
		local_pose_graph_no_filtering->add(prior_factor);

		isam2_graph.add(prior_factor);

		// add piror value使用current_symbol作为键，将pose_to作为值，插入到initial_values和isam2_initial_values中
		initial_values->insert(current_symbol, pose_to);
		isam2_initial_values.insert(current_symbol, pose_to);
		new_pose_to = pose_to;

		LOG(INFO) << "createPrior:[" << id_ << "]" << new_pose_to.translation().x() << " " << new_pose_to.translation().y()
				  << " " << new_pose_to.translation().z() << new_pose_to.rotation().roll() << " " << new_pose_to.rotation().pitch()
				  << " " << new_pose_to.rotation().yaw() << "." << endl;
	}
	// add odometry factor，已经不是第一帧了，添加里程计因子
	else
	{
		// incremental odom in local frame计算相对于本地坐标系的里程计增量。
		// 将keyposes_cloud_6d点云数据中的最后一个点转换为gtsam库中的Pose3类型的变量pose_from。
		auto pose_from = pclPointTogtsamPose3(keyposes_cloud_6d->points[poses_num - 1]);
		// 计算了从pose_from到pose_to之间的里程计增量，并将结果存储在名为pose_increment的变量中。
		auto pose_increment = pose_from.between(pose_to);
		// 用于表示之前的里程计的位姿
		Symbol previous_symbol = Symbol('a' + id_, poses_num - 1);
		// 获取 odometry_noise 对象的协方差矩阵，并将其存储在名为covariance的矩阵中。
		Matrix covariance = odometry_noise->covariance();

		// add odometry factor to graph
		// 创建新的位姿因子，使用betweenfactor类，该类包含（先前和当前位姿的符号，从pose_from到pose_to之间的里程计增量，里程计的噪声）
		NonlinearFactor::shared_ptr factor(new BetweenFactor<Pose3>(
			previous_symbol, current_symbol, pose_increment, odometry_noise));
		local_pose_graph->add(factor);
		local_pose_graph_no_filtering->add(factor);
		isam2_graph.add(factor);

		// add odometry value
		// 将当前位姿的初始值插入到isam2的初始值集合（isam2_initial_values）中。
		isam2_initial_values.insert(current_symbol, pose_to);
		// incremental odom in global graph frame
		// 获取先前位姿的初始值（pose_from）
		pose_from = initial_values->at<Pose3>(previous_symbol);
		// 当前位姿（new_pose_to）=先前位姿 * 里程计增量
		new_pose_to = pose_from * pose_increment;
		// 将新的当前位姿（new_pose_to）插入到全局图的初始值中（initial_values）。
		initial_values->insert(current_symbol, new_pose_to);

		// save factor in local map (for PCM)
		// boost::dynamic_pointer_cast是一个类型转换函数，用于将因子指针转换为特定类型的指针。
		auto new_factor = boost::dynamic_pointer_cast<BetweenFactor<Pose3>>(factor);
		// 将new_factor和协方差值covariance添加到机器人的本地地图中
		robot_local_map.addTransform(*new_factor, covariance);

		LOG(INFO) << "createOdom:[" << id_ << "]"
				  << "[" << poses_num - 1 << "-" << poses_num << "]--["
				  << pose_from.translation().x() << " " << pose_from.translation().y() << " " << pose_from.translation().z()
				  << pose_from.rotation().roll() << " " << pose_from.rotation().pitch() << " " << pose_from.rotation().yaw() << "],["
				  << new_pose_to.translation().x() << " " << new_pose_to.translation().y() << " " << new_pose_to.translation().z()
				  << new_pose_to.rotation().roll() << " " << new_pose_to.rotation().pitch() << " " << new_pose_to.rotation().yaw() << "]." << endl;
	}

	// optimizing
	// isam2_graph.print("GTSAM Graph:\n");
	// 使用isam2_graph（放的是factor）和isam2_initial_values（初始值）进行更新isam2
	isam2->update(isam2_graph, isam2_initial_values);
	// 清空图中的变量和约束。
	isam2_graph.resize(0);
	isam2_initial_values.clear();
	// 执行了isam2对象的calculateEstimate()方法，用以计算当前估计。
	isam2_current_estimates = isam2->calculateEstimate();
	// 从isam2_current_estimates中取出特定符号current_symbol对应的估计值，并将其存储到isam2_keypose_estimate变量中
	isam2_keypose_estimate = isam2_current_estimates.at<Pose3>(current_symbol);

	// save pose in local frame
	// 将isam2_keypose_estimate估计出来得到姿态信息保存到keyposes_cloud_3d和keyposes_cloud_6d中
	static PointPose3D pose_3d;
	pose_3d.x = isam2_keypose_estimate.translation().x();
	pose_3d.y = isam2_keypose_estimate.translation().y();
	pose_3d.z = isam2_keypose_estimate.translation().z();
	pose_3d.intensity = poses_num; // keyframe index
	keyposes_cloud_3d->push_back(pose_3d);

	static PointPose6D pose_6d;
	pose_6d.x = pose_3d.x;
	pose_6d.y = pose_3d.y;
	pose_6d.z = pose_3d.z;
	pose_6d.intensity = pose_3d.intensity;
	pose_6d.roll = isam2_keypose_estimate.rotation().roll();
	pose_6d.pitch = isam2_keypose_estimate.rotation().pitch();
	pose_6d.yaw = isam2_keypose_estimate.rotation().yaw();
	pose_6d.time = robots[id_].time_cloud_input; // keyframe timestamp
	keyposes_cloud_6d->push_back(pose_6d);

	LOG(INFO) << "save:[" << id_ << "]--[" << poses_num << "]--" << isam2_keypose_estimate.translation().x()
			  << " " << isam2_keypose_estimate.translation().y() << " " << isam2_keypose_estimate.translation().z()
			  << " " << isam2_keypose_estimate.rotation().roll() << " " << isam2_keypose_estimate.rotation().pitch()
			  << " " << isam2_keypose_estimate.rotation().yaw() << "." << endl;

	// save path for visualization
	updateLocalPath(pose_6d);
	updateGlobalPath(new_pose_to);
}

// bool函数，判断是否需要保存关键帧，只保存第一帧和大于阈值的关键帧
bool distributedMapping::saveFrame(
	const Pose3 &pose_to)
{
	// 如果是第一个关键帧，直接保存
	if (keyposes_cloud_3d->empty())
	{
		return true;
	}
	// 使用last_keypose接收最后一个关键帧的gtsampos3类型的位姿
	auto last_keypose = pclPointTogtsamPose3(keyposes_cloud_6d->back());
	auto pose_increment = last_keypose.inverse() * pose_to;

	float x = pose_increment.translation().x();
	float y = pose_increment.translation().y();
	float z = pose_increment.translation().z();
	float roll = pose_increment.rotation().roll();
	float pitch = pose_increment.rotation().pitch();
	float yaw = pose_increment.rotation().yaw();

	// select keyframe 绝对值小于阈值的不保存
	if (abs(roll) < keyframe_angle_threshold_ && abs(pitch) < keyframe_angle_threshold_ &&
		abs(yaw) < keyframe_angle_threshold_ && sqrt(x * x + y * y + z * z) < keyframe_distance_threshold_)
	{
		return false;
	}

	return true;
}

// 更新分布式地图的局部路径信息
void distributedMapping::updateLocalPath(
	const PointPose6D &pose)
{
	// 静态变量，用于保存路径信息
	static geometry_msgs::PoseStamped pose_stamped_msg;
	pose_stamped_msg.header.stamp = ros::Time::now();
	pose_stamped_msg.header.frame_id = world_frame_;
	pose_stamped_msg.pose.position.x = pose.x;
	pose_stamped_msg.pose.position.y = pose.y;
	pose_stamped_msg.pose.position.z = pose.z;
	// 将根据pose的roll、pitch和yaw创建四元数q
	tf::Quaternion q = tf::createQuaternionFromRPY(pose.roll, pose.pitch, pose.yaw);
	pose_stamped_msg.pose.orientation.x = q.x();
	pose_stamped_msg.pose.orientation.y = q.y();
	pose_stamped_msg.pose.orientation.z = q.z();
	pose_stamped_msg.pose.orientation.w = q.w();
	// pose_stamped_msg添加到local_path.poses的末尾。
	local_path.poses.push_back(pose_stamped_msg);
}

// 分布式地图的位姿信息是否更新
bool distributedMapping::updatePoses()
{
	// 初始化返回值
	bool return_value = false;
	// 如果keyposes_cloud_3d为空，直接返回false
	if (keyposes_cloud_3d->empty())
	{
		return return_value;
	}
	// 在loop文件中void distributedMapping::calculateTransformation函数中修改intra_robot_loop_close_flag的值，表示是否检测到了内部机器人循环
	if (intra_robot_loop_close_flag)
	{
		// clear path 清空局部路径
		local_path.poses.clear();

		// aggregate estimates 集合估计
		for (const Values::ConstKeyValuePair &key_value : isam2_current_estimates)
		{
			// update key poses
			// 将当前元素中的键值赋给变量 key
			Symbol key = key_value.key;
			// 获取索引，赋值给变量index
			int index = key.index();
			Pose3 pose = isam2_current_estimates.at<Pose3>(key);

			keyposes_cloud_3d->points[index].x = pose.translation().x();
			keyposes_cloud_3d->points[index].y = pose.translation().y();
			keyposes_cloud_3d->points[index].z = pose.translation().z();

			keyposes_cloud_6d->points[index].x = keyposes_cloud_3d->points[index].x;
			keyposes_cloud_6d->points[index].y = keyposes_cloud_3d->points[index].y;
			keyposes_cloud_6d->points[index].z = keyposes_cloud_3d->points[index].z;
			keyposes_cloud_6d->points[index].roll = pose.rotation().roll();
			keyposes_cloud_6d->points[index].pitch = pose.rotation().pitch();
			keyposes_cloud_6d->points[index].yaw = pose.rotation().yaw();

			// 将更新后的位姿信息保存到局部路径中
			updateLocalPath(keyposes_cloud_6d->points[index]);
		}
		// 初始化机器人内部回环标志
		intra_robot_loop_close_flag = false;
		return_value = true;
		LOG(INFO) << "updatePoses:[" << id_ << "]." << endl;
	}

	// copy keyposes复制关键帧
	*copy_keyposes_cloud_3d = *keyposes_cloud_3d;
	*copy_keyposes_cloud_6d = *keyposes_cloud_6d;

	// save updated keypose
	isam2_keypose_estimate = pclPointTogtsamPose3(keyposes_cloud_6d->back());

	return return_value;
}

// 最终是吧求出的global_descriptor_msg全局描述子消息发布出去
void distributedMapping::makeDescriptors()
{
	// 全局描述子不为空时
	while (!store_descriptors.empty())
	{
		// 前端取出描述子数据和id
		auto msg_data = store_descriptors.front().second;
		auto msg_id = store_descriptors.front().first;
		store_descriptors.pop_front();
		// 将得到的数据给saveDescriptorAndKey，调用函数获得描述子和关键帧
		keyframe_descriptor->saveDescriptorAndKey(msg_data.values.data(), msg_id, msg_data.index);
	}
	// 若初始值为空、内部回环和外部回环都不使能，直接返回
	if (initial_values->empty() || (!intra_robot_loop_closure_enable_ && !inter_robot_loop_closure_enable_))
	{
		return;
	}

	// downsample keyframe降采样关键帧
	cloud_for_decript_ds->clear();
	// 判断雷达类型
	if (sensor_ == LiDARType::LIVOX)
	{
		// pcl::PointCloud<PointPose3D>::Ptr multiKeyFrameCloud(new pcl::PointCloud<PointPose3D>());
		// loopFindNearKeyframes(multiKeyFrameCloud, initial_values->size() - 1, history_keyframe_search_num_);
		// downsample_filter_for_descriptor.setInputCloud(multiKeyFrameCloud);
		downsample_filter_for_descriptor.setInputCloud(robots[id_].keyframe_cloud);
	}
	else
	{
		downsample_filter_for_descriptor.setInputCloud(robots[id_].keyframe_cloud);
	}
	// 将结果保存到cloud_for_decript_ds指向的点云中。
	downsample_filter_for_descriptor.filter(*cloud_for_decript_ds);

	// make and save global descriptors
	// 返回的是m2dp_vec或者descriptor_msg_data或者descriptor_data三种描述子向量中的一种
	auto descriptor_vec = keyframe_descriptor->makeAndSaveDescriptorAndKey(*cloud_for_decript_ds, id_, initial_values->size() - 1);
	LOG(INFO) << "makeDescriptors[" << id_ << "]." << endl;

	// extract descriptors values
	// 全局描述子消息
	global_descriptor_msg.values.swap(descriptor_vec);
	// keyfame index
	// 索引和header的时间戳
	global_descriptor_msg.index = initial_values->size() - 1;
	global_descriptor_msg.header.stamp = robots[id_].time_cloud_input_stamp;
	// publish message
	robots[id_].pub_descriptors.publish(global_descriptor_msg);
}

// 根据机器人编号发布全局路径和局部路径
void distributedMapping::publishPath()
{
	// publish path
	if (pub_global_path.getNumSubscribers() != 0)
	{
		global_path.header.stamp = robots[id_].time_cloud_input_stamp;
		global_path.header.frame_id = world_frame_;
		pub_global_path.publish(global_path);
	}

	if (pub_local_path.getNumSubscribers() != 0)
	{
		local_path.header.stamp = robots[id_].time_cloud_input_stamp;
		local_path.header.frame_id = robots[id_].odom_frame_;
		pub_local_path.publish(local_path);
	}
}

// 输入当前帧时间戳，发布一个tf::StampedTransform对象其中包含world_to_odom的变换信息，以及时间戳和world_frame_、robots[id_]的odom_frame_
void distributedMapping::publishTransformation(
	const ros::Time &timestamp)
{
	// 设置静态的tf，发布从世界坐标系到里程计坐标系的变换关系
	static tf::TransformBroadcaster world_to_odom_tf_broadcaster;
	static Symbol first_key((id_ + 'a'), 0);

	// 获取第一个关键帧的位姿
	Pose3 first_pose = initial_values->at<Pose3>(first_key);
	// 获取到之前记录的robots[id_]的prior_odom（前一次的里程计）的Pose3对象old_first_pose。
	Pose3 old_first_pose = robots[id_].piror_odom;
	// 前一次和当前之间的姿态之间的差异pose_between
	Pose3 pose_between = first_pose * old_first_pose.inverse();
	// 创建world_to_odom表示旋转和平移对象
	tf::Transform world_to_odom = tf::Transform(tf::createQuaternionFromRPY(
													pose_between.rotation().roll(), pose_between.rotation().pitch(), pose_between.rotation().yaw()),
												tf::Vector3(pose_between.translation().x(), pose_between.translation().y(), pose_between.translation().z()));
	// 发送一个tf::StampedTransform对象，其中包含world_to_odom的变换信息，以及时间戳和world_frame_、robots[id_]的odom_frame_作为帧名字
	world_to_odom_tf_broadcaster.sendTransform(tf::StampedTransform(world_to_odom, timestamp, world_frame_, robots[id_].odom_frame_));
}

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
	class distributedMapping: distributed mapping
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
// 返回标志位all_neighbor_started（邻居机器人是否开始优化）和sent_start_optimization_flag（是否准备好进行优化。）的与  运算结果
bool distributedMapping::startOptimizationCondition()
{
	// check if all neighbors started
	// 邻居是否开始优化。
	bool all_neighbor_started = !neighbors_started_optimization.empty();
	for (const auto &neighbor_started : neighbors_started_optimization)
	{
		all_neighbor_started &= neighbor_started.second && (neighbor_state[neighbor_started.first] <= OptimizerState::Start);
	}

	return all_neighbor_started && sent_start_optimization_flag;
}

// updateOptimizer();：调用这个函数，将图和值加载到优化器中
void distributedMapping::updateOptimizer()
{
	// load subgraphs
	// 将本地位姿图 local_pose_graph 和初始值 initial_values 组合成一个数据对。
	graph_values_vec = std::make_pair(local_pose_graph, initial_values);
	// 调用优化器的 loadSubgraphAndCreateSubgraphEdge 函数，将子图和初始值加载到优化器中。
	optimizer->loadSubgraphAndCreateSubgraphEdge(graph_values_vec);

	// add prior to the first robot
	// 找到第一个机器人（拥有最低的 lowest_id_included）并将其 ID 保存为 prior_owner。
	std::pair<int, int> robot_pair = std::make_pair(id_, lowest_id_included);
	for (const auto &neighbor_lowest_id : neighbors_lowest_id_included)
	{
		// 判断当前邻居是否在 neighbors_within_communication_range 集合中，即是否处于通信范围内。
		// 通信范围内的邻居机器人的 ID 集合。
		// 总之，找到通讯范围内的最好的机器人，作为先验机器人。
		if (neighbors_within_communication_range.find(neighbor_lowest_id.first) != neighbors_within_communication_range.end())
		{
			if (neighbor_lowest_id.second < robot_pair.second)
			{
				robot_pair = neighbor_lowest_id;
				if (lowest_id_to_included > neighbor_lowest_id.second)
				{
					lowest_id_to_included = neighbor_lowest_id.second;
				}
			}
			if (neighbor_lowest_id.second == robot_pair.second)
			{
				if (robot_pair.first > neighbor_lowest_id.first)
				{
					robot_pair = neighbor_lowest_id;
				}
			}
		}
	}
	// first robot always have prior
	prior_owner = robot_pair.first;
	ROS_DEBUG("priorOwner<%d> %d", id_, prior_owner);
	if (robot_pair.first == id_)
	{
		static Key prior_key = Symbol('a' + id_, 0);
		optimizer->addPrior(prior_key, robots[id_].piror_odom, prior_noise);
		prior_added = true;
	}

	// check for graph connectivity
	neighboring_robots = optimizer->getNeighboringChars();
	if (neighboring_robots.size() > 0)
	{
		graph_disconnected = false;
	}
	bool has_separator_with_neighbor = false;
	// check neighbor within communication range
	for (const auto &neighbor : neighbors_within_communication_range)
	{
		if (neighboring_robots.find((char)(neighbor + 'a')) != neighboring_robots.end())
		{
			has_separator_with_neighbor = true;
			neighbors_estimation_done[neighbor] = false;
			neighbors_pose_estimate_finished[neighbor] = false;
			neighbors_rotation_estimate_finished[neighbor] = false;
		}
	}
	if (!has_separator_with_neighbor)
	{
		changeOptimizerState(OptimizerState::Idle);
	}
}

void distributedMapping::outliersFiltering()
{
	robot_local_map_backup = robot_local_map;

	if (use_pcm_)
	{
		measurements_accepted_num = 0;
		measurements_rejected_num = 0;
		// perform pairwise consistency maximization for each pair robot
		for (const auto &neighbor : neighbors_within_communication_range)
		{
			if (neighbor == id_ || pose_estimates_from_neighbors.find(neighbor) == pose_estimates_from_neighbors.end())
			{
				continue;
			}

			// get other robot trajectory
			graph_utils::Transforms empty_transforms;
			auto neighbor_local_info = robot_measurements::RobotLocalMap(
				pose_estimates_from_neighbors.at(neighbor), empty_transforms, robot_local_map.getLoopClosures());

			// get inter-robot loop closure measurements
			graph_utils::Transforms loop_closures_transforms;
			for (const auto &transform : robot_local_map.getTransforms().transforms)
			{
				auto robot0 = (int)(Symbol(transform.second.i).chr() - 97);
				auto robot1 = (int)(Symbol(transform.second.j).chr() - 97);
				if (robot0 == neighbor || robot1 == neighbor)
				{
					loop_closures_transforms.transforms.insert(transform);
				}
			}
			auto inter_robot_measurements = robot_measurements::InterRobotMeasurements(
				loop_closures_transforms, optimizer->robotName(), (char)neighbor + 97);

			// local trajectory
			auto local_info = robot_measurements::RobotLocalMap(
				pose_estimates_from_neighbors.at(id_), robot_local_map.getTransforms(), robot_local_map.getLoopClosures());

			// create global map
			auto globalMap = global_map::GlobalMap(local_info, neighbor_local_info,
												   inter_robot_measurements, pcm_threshold_, use_heuristics_);

			// pairwise consistency maximization
			auto max_clique_info = globalMap.pairwiseConsistencyMaximization();
			std::vector<int> max_clique = max_clique_info.first;
			measurements_accepted_num += max_clique.size();
			measurements_rejected_num += max_clique_info.second;

			// retrieve indexes of rejected measurements
			auto loopclosures_ids = optimizer->loopclosureEdge();
			std::vector<int> rejected_loopclosure_ids;
			rejected_loopclosure_ids.reserve(1000);
			std::set<std::pair<Key, Key>> accepted_key_pairs, rejected_key_pairs;
			for (int i = 0; i < loopclosures_ids.size(); i++)
			{
				if (distributed_pcm::DistributedPCM::isloopclosureToBeRejected(max_clique, loopclosures_ids[i],
																			   loop_closures_transforms, inter_robot_measurements.getLoopClosures(), optimizer))
				{
					rejected_loopclosure_ids.emplace_back(i);

					// Update robot local map and store keys
					auto loopclosure_factor = boost::dynamic_pointer_cast<BetweenFactor<Pose3>>(
						optimizer->currentGraph().at(loopclosures_ids[i]));
					robot_local_map.removeTransform(std::make_pair(loopclosure_factor->keys().at(0), loopclosure_factor->keys().at(1)));
					optimizer->eraseseparatorsSymbols(std::make_pair(loopclosure_factor->keys().at(0), loopclosure_factor->keys().at(1)));
					optimizer->eraseseparatorsSymbols(std::make_pair(loopclosure_factor->keys().at(1), loopclosure_factor->keys().at(0)));
					rejected_key_pairs.insert(std::make_pair(loopclosure_factor->keys().at(0), loopclosure_factor->keys().at(1)));
				}
				else
				{
					auto loopclosure_factor = boost::dynamic_pointer_cast<BetweenFactor<Pose3>>(
						optimizer->currentGraph().at(loopclosures_ids[i]));
					accepted_key_pairs.insert(std::make_pair(loopclosure_factor->keys().at(0), loopclosure_factor->keys().at(1)));
				}
			}

			// remove measurements not in the max clique
			int number_loopclosure_ids_removed = 0;
			for (const auto &index : rejected_loopclosure_ids)
			{
				auto factor_id = loopclosures_ids[index] - number_loopclosure_ids_removed;
				number_loopclosure_ids_removed++;
				optimizer->eraseFactor(factor_id);
				local_pose_graph->erase(local_pose_graph->begin() + factor_id);
			}

			// Update loopclosure ids
			std::vector<size_t> new_loopclosure_ids;
			new_loopclosure_ids.reserve(10000);
			int number_of_edges = optimizer->currentGraph().size();
			for (int i = 0; i < number_of_edges; i++)
			{
				auto keys = optimizer->currentGraph().at(i)->keys();
				if (keys.size() == 2)
				{
					char robot0 = symbolChr(keys.at(0));
					char robot1 = symbolChr(keys.at(1));
					int index0 = symbolIndex(keys.at(0));
					int index1 = symbolIndex(keys.at(1));
					if (robot0 != robot1)
					{
						new_loopclosure_ids.push_back(i);
					}
				}
			}
			optimizer->setloopclosureIds(new_loopclosure_ids);

			// save accepted pair
			for (const auto &accepted_pair : accepted_key_pairs)
			{
				accepted_keys.insert(accepted_pair);
				if (Symbol(accepted_pair.first).chr() == char(id_ + 'a'))
				{
					other_robot_keys_for_optimization.insert(accepted_pair.second);
				}
				else
				{
					other_robot_keys_for_optimization.insert(accepted_pair.first);
				}
			}
			// save rejected pair
			for (const auto &rejected_pair : rejected_key_pairs)
			{
				rejected_keys.insert(rejected_pair);
			}

			ROS_DEBUG("outliersFiltering<%d>, other=%d, max clique size=%d, removed=%d", id_, neighbor,
					  max_clique.size(), max_clique_info.second);
		}
	}
}

void distributedMapping::computeOptimizationOrder()
{
	optimization_order.clear();
	optimization_order.emplace_back(prior_owner);
	std::map<int, int> edges_num;
	auto robots_consider = neighbors_within_communication_range;
	robots_consider.insert(id_);
	while (true)
	{
		edges_num.clear();
		for (const auto &robot0 : robots_consider)
		{
			if (std::find(optimization_order.begin(), optimization_order.end(), robot0) == optimization_order.end())
			{
				// for each robot not in the ordering, compute the number of edges
				// towards robots inside the ordering and select the one with the largest number
				int robot_edges_num = 0;
				for (size_t robot1 : optimization_order)
				{
					robot_edges_num += adjacency_matrix(robot0, robot1);
				}
				if (robot_edges_num != 0)
				{
					edges_num.insert(std::make_pair(robot0, robot_edges_num));
				}
			}
		}
		if (edges_num.empty())
		{
			break;
		}
		int maximum_edge_num = -1;
		int maximum_robot = -1;
		for (const auto &robot_info : edges_num)
		{
			if (robot_info.second > maximum_edge_num)
			{
				maximum_edge_num = robot_info.second;
				maximum_robot = robot_info.first;
			}
		}
		optimization_order.emplace_back(maximum_robot);
		edges_num.erase(maximum_robot);
	}

	in_order = false;
	for (int i = 0; i < optimization_order.size(); i++)
	{
		if (optimization_order[i] == id_)
		{
			in_order = true;
		}
	}
}

void distributedMapping::initializePoseGraphOptimization()
{
	// load graph&value and check graph connectivity
	updateOptimizer();

	// fliter outlier with distributed PCM method
	/* 使用分布式 PCM 方法过滤离群值
	调用这个函数，识别并移除异常数据，
	以提高优化结果的准确性 */
	outliersFiltering();

	// ordering		
	/* 		调用这个函数，根据某种策略计算优
	化的顺序，例如根据机器人 ID 进行排序。 */

	computeOptimizationOrder();

	// initialize
	optimizer->updateInitialized(false);
	optimizer->clearNeighboringRobotInit();
	rotation_estimate_finished = false;
	pose_estimate_finished = false;
	estimation_done = false;
}

// 分布式建图系统中担任协调多个机器人进行旋转估计的角色。
bool distributedMapping:: rotationEstimationStoppingBarrier()
{
	// check neighbor state
	/* 检查邻居状态:
	函数先判断是否轮到当前机器人进行旋转估计 (in_turn 变量)。
	它通过检查所有邻居机器人的状态，确保它们也处于旋转估计阶段。 */
	bool in_turn = true;
	neighboring_robots = optimizer->getNeighboringChars();
	for (int i = 0; i < optimization_order.size(); i++)
	{
		if (optimization_order[i] != id_ &&
			neighboring_robots.find((char)(optimization_order[i] + 'a')) != neighboring_robots.end())
		{
			in_turn &= (neighbor_state[optimization_order[i]] == OptimizerState::RotationEstimation);
		}
	}
	// send rotation estimate to the pre-order robot
	/* 发送估计信息:
	如果轮到当前机器人进行估计，并且还未开始发送估计信息：
	清空缓存。
	提取所有回环的旋转估计。
	为每个邻居机器人构造估计信息。
	发送估计信息给所有邻居机器人。 */
	if (in_turn && !rotation_estimate_start)
	{
		rotation_estimate_start = true;
		// clear buffer
		for (const auto &neighbor : neighbors_within_communication_range)
		{
			robots[neighbor].estimate_msg.pose_id.clear();
			robots[neighbor].estimate_msg.estimate.clear();
		}
		// extract rotation estimate for each loop closure
		for (const std::pair<Symbol, Symbol> &separator_symbols : optimizer->separatorsSymbols())
		{
			// robot id
			int other_robot = (int)(separator_symbols.first.chr() - 'a');
			// pose id
			robots[other_robot].estimate_msg.pose_id.push_back(separator_symbols.second.index());
			// rotation estimates
			Vector rotation_estimate = optimizer->linearizedRotationAt(separator_symbols.second.key());
			for (int it = 0; it < 9; it++)
			{
				robots[other_robot].estimate_msg.estimate.push_back(rotation_estimate[it]);
			}
		}
		// send rotation estimate
		for (int i = 0; i < optimization_order.size(); i++)
		{
			int other_robot = optimization_order[i];
			if (other_robot == id_)
			{
				break;
			}

			robots[other_robot].estimate_msg.initialized = optimizer->isRobotInitialized();
			robots[other_robot].estimate_msg.receiver_id = other_robot;
			robots[other_robot].estimate_msg.estimation_done = estimation_done;
			robots[id_].pub_neighbor_rotation_estimates.publish(robots[other_robot].estimate_msg);
		}
	}

	// check if neighbors have begun pose estimation
	/* 检查邻居是否已开始姿态估计：
	遍历所有通信范围内的邻居机器人。
	如果所有邻居都已进入姿态估计阶段，则返回 true，表示可以停止旋转估计。 */
	bool all_finished_rotation_estimation = true;
	for (const auto &neighbor : neighbors_within_communication_range)
	{
		all_finished_rotation_estimation &= (neighbor_state[neighbor] == OptimizerState::PoseEstimation);
	}
	if (all_finished_rotation_estimation)
	{
		return true;
	}

	// neighbors have finished rotation estimation
	/* 检查邻居是否已完成旋转估计：
	初始化一个变量 stop_rotation_estimation，表示是否停止旋转估计。
	遍历所有优化顺序中的机器人：
	如果其他机器人的旋转估计已完成，或者其状态比当前机器人更高级，则将 stop_rotation_estimation 置为 true */
	bool stop_rotation_estimation = rotation_estimate_finished;
	for (int i = 0; i < optimization_order.size(); i++)
	{
		int otherRobot = optimization_order[i];
		if (otherRobot != id_)
		{
			stop_rotation_estimation &= neighbors_rotation_estimate_finished[otherRobot] ||
										neighbor_state[otherRobot] > optimizer_state;
		}
	}
	return stop_rotation_estimation;
}

// 中止分布式建图系统中的优化过程，并恢复到之前的备份状态。
void distributedMapping::abortOptimization(
	const bool &log_info)
{
	/* 
	恢复备份的位姿变换:
	遍历备份地图 (robot_local_map_backup) 中的所有位姿变换。
	对于每个变换：
		如果当前地图 (robot_local_map) 中没有这个变换，则将其恢复：
			创建噪声模型。
			构造位姿因子 (BetweenFactor<Pose3>)。
			添加位姿因子到当前地图和局部位姿图 (local_pose_graph) 中。
	 */
	for (const auto &transform : robot_local_map_backup.getTransforms().transforms)
	{
		if (robot_local_map.getTransforms().transforms.find(transform.first) == robot_local_map.getTransforms().transforms.end())
		{
			SharedNoiseModel model = noiseModel::Gaussian::Covariance(transform.second.pose.covariance_matrix);
			auto factor = BetweenFactor<Pose3>(
				transform.second.i, transform.second.j, transform.second.pose.pose, model);
			robot_local_map.addTransform(factor, transform.second.pose.covariance_matrix);
			local_pose_graph->push_back(factor);
		}
	}

	changeOptimizerState(OptimizerState::Idle);
}

// 在分布式建图系统中负责移除非活跃的邻居机器人，并在必要时中止优化过程。
void distributedMapping::removeInactiveNeighbors()
{
	std::vector<int> removed_neighbors;
	removed_neighbors.reserve(1000);
	/* 
	识别非活跃邻居：
		创建一个列表 removed_neighbors 来存储要移除的邻居。
		遍历所有通信范围内的邻居：
		如果邻居状态处于 OptimizerState::Idle，表示该邻居不再活跃，将其添加到 removed_neighbors 列表中。 */
	for (const auto &neighbor : neighbors_within_communication_range)
	{
		if (neighbor_state[neighbor] == OptimizerState::Idle)
		{
			removed_neighbors.emplace_back(neighbor);
		}
	}
	/* 移除邻居信息：
	遍历 removed_neighbors 列表：
	从以下记录中移除该邻居的信息：
		neighbors_within_communication_range：通信范围内的邻居列表。
		neighbors_rotation_estimate_finished：邻居是否完成旋转估计的标记。
		neighbors_pose_estimate_finished：邻居是否完成姿态估计的标记。
		neighbors_estimation_done：邻居是否完成估计的标记。
		neighbors_lowest_id_included：邻居是否被包含在优化中的标记。 */
	for (const auto &neighbor : removed_neighbors)
	{
		neighbors_within_communication_range.erase(neighbor);
		;
		neighbors_rotation_estimate_finished.erase(neighbor);
		neighbors_pose_estimate_finished.erase(neighbor);
		neighbors_estimation_done.erase(neighbor);
		neighbors_lowest_id_included.erase(neighbor);
	}
	/* 
	检查是否所有邻居都非活跃：
	如果 neighbors_within_communication_range 为空，表示没有活跃邻居了。
	记录日志信息。
	调用 abortOptimization() 函数中止优化。 */
	if (neighbors_within_communication_range.empty())
	{
		ROS_DEBUG("Stop optimization<%d>, there are inactive neighbors.", id_);
		abortOptimization(false);
	}
}
// 分布式建图系统中担任保障机制，用于检查优化过程是否陷入停滞，并在必要时进行干预。
void distributedMapping::failSafeCheck()
{
	/* 检查优化进展：
	比较当前优化器的最新变化 (optimizer->latestChange()) 是否与记录的上一次变化 (latest_change) 一致。
		如果一致，表示优化没有进展，将无变化步数 steps_without_change 加 1。
		否则，将 steps_without_change 重置为 0，并更新 latest_change。 */
	if (latest_change == optimizer->latestChange())
	{
		steps_without_change++;
	}
	else
	{
		steps_without_change = 0; 
		latest_change = optimizer->latestChange();
	}

	// wait enough time to receive data from neighbors
	/* 
	判断是否超出无变化阈值：
	如果 steps_without_change 超过了预设的无变化步数阈值 (fail_safe_steps_) 乘以通信范围内邻居数量 (neighbors_within_communication_range.size())，表示优化可能陷入停滞。
	记录日志信息。
	调用 abortOptimization() 函数中止优化。
	 */
	if (steps_without_change > fail_safe_steps_ * neighbors_within_communication_range.size())
	{
		ROS_DEBUG("No progress<%d>, Stop optimization", id_);
		abortOptimization(false);
	}
}

// 在分布式建图系统中负责初始化姿态估计，为后续的优化过程做准备。
void distributedMapping::initializePoseEstimation()
{
	/* 
	转换线性化旋转：
	调用 optimizer->convertLinearizedRotationToPoses() 函数将预估的线性化旋转转换为完整的位姿。 */
	optimizer->convertLinearizedRotationToPoses();
	Values neighbors = optimizer->neighbors();
	/* 处理邻居位姿：
	获取所有邻居机器人的位姿估计值。
	遍历每个邻居的估计值：
		提取线性化旋转。
		将线性化旋转转换为完整的位姿（只包含旋转，平移置为零）。
		将完整的位姿更新到优化器中。 */
	for (const Values::ConstKeyValuePair &key_value : neighbors)
	{
		Key key = key_value.key;
		// pick linear rotation estimate
		VectorValues neighbor_estimate_rot_lin;
		neighbor_estimate_rot_lin.insert(key, optimizer->neighborsLinearizedRotationsAt(key));
		// make a pose out of it
		Values neighbor_rot_estimate =
			InitializePose3::normalizeRelaxedRotations(neighbor_estimate_rot_lin);
		Values neighbor_pose_estimate =
			distributed_mapper::evaluation_utils::pose3WithZeroTranslation(neighbor_rot_estimate);
		// store it
		optimizer->updateNeighbor(key, neighbor_pose_estimate.at<Pose3>(key));
	}

	// reset flags for flagged initialization.
	/* 重置估计状态：
	将优化器的初始化标志重置为未初始化。
	清除邻居机器人的初始化信息。
	将所有估计完成标志重置为未完成。
	重置优化器的最新变化记录。 */
	optimizer->updateInitialized(false);
	optimizer->clearNeighboringRobotInit();
	estimation_done = false;
	for (auto &neighbor_done : neighbors_estimation_done)
	{
		neighbor_done.second = false;
	}
	optimizer->resetLatestChange();
}

// 负责协调多个机器人的姿态估计过程
bool distributedMapping::poseEstimationStoppingBarrier()
{
	// check neighbor state
	/* 检查邻居状态：
	检查是否轮到当前机器人进行姿态估计 (in_turn 变量)。
	它通过检查所有邻居机器人的状态，确保它们也处于姿态估计阶段或更高级的阶段。 */
	bool in_turn = true;
	// neighboring_robots = optimizer->getNeighboringChars();
	/* 发送估计信息：
	如果轮到当前机器人进行估计，并且还未开始发送估计信息：
		清空缓存。
		提取所有回环的姿态估计。
		为每个邻居机器人构造估计信息，包括位姿 ID、估计值、锚点偏移量等。
		发送估计信息给所有邻居机器人。 */
	for (int i = 0; i < optimization_order.size(); i++)
	{
		if (optimization_order[i] != id_ &&
			neighbors_within_communication_range.find(optimization_order[i]) != neighbors_within_communication_range.end())
		{
			in_turn &= (neighbor_state[optimization_order[i]] == OptimizerState::PoseEstimation);
		}
	}
	// send pose estimate to the pre-order robot
	if (in_turn && !pose_estimate_start)
	{
		pose_estimate_start = true;
		// clear buffer
		for (const auto &neighbor : neighbors_within_communication_range)
		{
			robots[neighbor].estimate_msg.pose_id.clear();
			robots[neighbor].estimate_msg.estimate.clear();
			robots[neighbor].estimate_msg.anchor_offset.clear();
		}
		// extract pose estimate from each loop closure
		for (const std::pair<Symbol, Symbol> &separator_symbols : optimizer->separatorsSymbols())
		{
			int other_robot = (int)(separator_symbols.first.chr() - 'a');

			robots[other_robot].estimate_msg.pose_id.push_back(separator_symbols.second.index());

			Vector pose_estimate = optimizer->linearizedPosesAt(separator_symbols.second.key());
			for (int it = 0; it < 6; it++)
			{
				robots[other_robot].estimate_msg.estimate.push_back(pose_estimate[it]);
			}
		}
		// send pose estimate
		for (int i = 0; i < optimization_order.size(); i++)
		{
			int other_robot = optimization_order[i];
			if (other_robot == id_)
			{
				break;
			}

			for (int i = 0; i < 3; i++)
			{
				robots[other_robot].estimate_msg.anchor_offset.push_back(anchor_offset.vector()[i]);
			}
			robots[other_robot].estimate_msg.initialized = optimizer->isRobotInitialized();
			robots[other_robot].estimate_msg.receiver_id = other_robot;
			robots[other_robot].estimate_msg.estimation_done = pose_estimate_finished;
			robots[id_].pub_neighbor_pose_estimates.publish(robots[other_robot].estimate_msg);
		}
	}

	// check if others have ended optimization
	bool all_finished_pose_estimation = true;
	for (const auto &neighbor : neighbors_within_communication_range)
	{
		bool other_robot_finished = (neighbor_state[neighbor] != OptimizerState::PoseEstimation) &&
									(neighbor_state[neighbor] != OptimizerState::PoseEstimationInitialization) &&
									(neighbor_state[neighbor] != OptimizerState::RotationEstimation);
		all_finished_pose_estimation &= other_robot_finished;
	}
	if (all_finished_pose_estimation)
	{
		return true;
	}

	// neighbors have finished pose estimation
	bool stop_pose_estimation = pose_estimate_finished;
	for (int i = 0; i < optimization_order.size(); i++)
	{
		int other_robot = optimization_order[i];
		if (other_robot != id_)
		{
			stop_pose_estimation &= neighbors_pose_estimate_finished[other_robot] ||
									neighbor_state[other_robot] > optimizer_state;
		}
	}
	return stop_pose_estimation;
}

// 同理更新全局位姿，并传递给global_path
void distributedMapping::updateGlobalPath(
	const Pose3 &pose)
{
	static geometry_msgs::PoseStamped pose_stamped_msg;
	pose_stamped_msg.header.stamp = ros::Time::now();
	pose_stamped_msg.header.frame_id = world_frame_;
	pose_stamped_msg.pose.position.x = pose.translation().x();
	pose_stamped_msg.pose.position.y = pose.translation().y();
	pose_stamped_msg.pose.position.z = pose.translation().z();
	pose_stamped_msg.pose.orientation.x = pose.rotation().toQuaternion().x();
	pose_stamped_msg.pose.orientation.y = pose.rotation().toQuaternion().y();
	pose_stamped_msg.pose.orientation.z = pose.rotation().toQuaternion().z();
	pose_stamped_msg.pose.orientation.w = pose.rotation().toQuaternion().w();

	global_path.poses.push_back(pose_stamped_msg);
}

void distributedMapping::incrementalInitialGuessUpdate()
{
	// update poses values将优化器的当前估计值更新到 initial_values 对象中，用于存储初始位姿估计。
	initial_values->update(optimizer->currentEstimate());

	// incremental update增量更新（Incremental Update）
	/* 遍历局部位姿图（local_pose_graph）中的所有因子：
	检查因子是否为 BetweenFactor<Pose3> 类型（表示两个位姿之间的相对约束）。
	获取因子涉及的两个位姿的 ID 和所属机器人 ID。
	判断如果第二个位姿不在优化器的估计值中，且两个位姿属于同一机器人且索引连续，则：
		获取第一个位姿的估计值。
		根据因子的测量值计算第二个位姿的估计值。
		更新 initial_values 中第二个位姿的估计值。 */
	for (size_t k = 0; k < local_pose_graph->size(); k++)
	{
		KeyVector keys = local_pose_graph->at(k)->keys();
		if (keys.size() == 2)
		{
			auto between_factor = boost::dynamic_pointer_cast<BetweenFactor<Pose3>>(local_pose_graph->at(k));
			Symbol key0 = between_factor->keys().at(0);
			Symbol key1 = between_factor->keys().at(1);
			int index0 = key0.index();
			int robot0 = key0.chr();
			int index1 = key1.index();
			int robot1 = key1.chr();
			if (!optimizer->currentEstimate().exists(key1) && (robot0 == robot1) && (index1 = index0 + 1))
			{
				// get previous pose
				auto previous_pose = initial_values->at<Pose3>(key0);
				// compose previous pose and measurement
				auto current_pose = previous_pose * between_factor->measured();
				// update pose in initial guess
				initial_values->update(key1, current_pose);
			}
		}
	}

	// aggregate estimates聚合估计
	global_path.poses.clear();
	/* 清空全局路径（global_path）和点云（poses_3d_cloud）。
	遍历 initial_values 中的所有位姿估计值：
	更新关键位姿（key poses）。
	将位姿转换为 3D 点云格式并添加到 poses_3d_cloud 中。
	更新全局路径（global_path）。
	*/
	pcl::PointCloud<PointPose3D>::Ptr poses_3d_cloud(new pcl::PointCloud<PointPose3D>());
	for (const Values::ConstKeyValuePair &key_value : *initial_values)
	{
		// update key poses
		Symbol key = key_value.key;
		int index = key.index();
		Pose3 pose = initial_values->at<Pose3>(key);

		PointPose3D pose_3d;
		pose_3d.x = pose.translation().x();
		pose_3d.y = pose.translation().y();
		pose_3d.z = pose.translation().z();
		pose_3d.intensity = key.index();
		poses_3d_cloud->push_back(pose_3d);

		updateGlobalPath(pose);
	}

	if (pub_keypose_cloud.getNumSubscribers() != 0)
	{
		// publish global map
		sensor_msgs::PointCloud2 keypose_cloud_msg;
		pcl::toROSMsg(*poses_3d_cloud, keypose_cloud_msg);
		keypose_cloud_msg.header.stamp = ros::Time::now();
		keypose_cloud_msg.header.frame_id = world_frame_;
		pub_keypose_cloud.publish(keypose_cloud_msg);
	}

	if (pub_global_path.getNumSubscribers() != 0)
	{
		global_path.header.stamp = ros::Time::now();
		global_path.header.frame_id = world_frame_;
		pub_global_path.publish(global_path);
	}
}
// 在分布式建图系统中负责结束优化过程，并更新相关信息。
void distributedMapping::endOptimization()
{incrementalInitialGuessUpdate()：
更新当前优化器估计值、遍历局部位姿图中的所有因子，获取因子涉及到的两个位姿的机器人。
优化路径和点云。
	incrementalInitialGuessUpdate();

	lowest_id_included = lowest_id_to_included;
}

// 改变目前的状态
void distributedMapping::changeOptimizerState(
	const OptimizerState &state)
{
	switch (state)
	{
	case OptimizerState::Idle:
		ROS_DEBUG("<%d>Idle", id_);
		break;
	case OptimizerState::Start:
		ROS_DEBUG("<%d>Start", id_);
		break;
	case OptimizerState::Initialization:
		ROS_DEBUG("<%d>Initialization", id_);
		break;
	case OptimizerState::RotationEstimation:
		ROS_DEBUG("<%d>RotationEstimation", id_);
		break;
	case OptimizerState::PoseEstimationInitialization:
		ROS_DEBUG("<%d>PoseEstimationInitialization", id_);
		break;
	case OptimizerState::PoseEstimation:
		ROS_DEBUG("<%d>PoseEstimation", id_);
		break;
	case OptimizerState::End:
		ROS_DEBUG("<%d>End", id_);
		break;
	case OptimizerState::PostEndingCommunicationDelay:
		ROS_DEBUG("<%d>PostEndingCommunicationDelay", id_);
		break;
	}

	optimizer_state = state;
	state_msg.data = (int)optimizer_state;
	robots[id_].pub_optimization_state.publish(state_msg);
}

// 根据不同的 optimizer_state，执行不同的操作
void distributedMapping::run(
	const ros::TimerEvent &)
{
	// update optimizer state
	switch (optimizer_state)
	{
	case OptimizerState::Idle:
		if (startOptimizationCondition())
		{
			current_rotation_estimate_iteration = 0;
			current_pose_estimate_iteration = 0;
			// neighbors_within_communication_range.clear();
			neighbors_rotation_estimate_finished.clear();
			neighbors_pose_estimate_finished.clear();
			neighbors_anchor_offset.clear();
			neighbors_estimation_done.clear();
			latest_change = -1;
			steps_without_change = 0;
			lowest_id_to_included = lowest_id_included;
			neighbors_started_optimization.clear();
			if (prior_added)
			{
				optimizer->removePrior();
				prior_added = false;
			}
			optimization_steps = 0;
			changeOptimizerState(OptimizerState::Start);
		}
		else
		{
			changeOptimizerState(OptimizerState::Idle);
		}
		state_msg.data = (int)optimizer_state;
		robots[id_].pub_optimization_state.publish(state_msg);
		break;

	case OptimizerState::Start:
		// 初始化图优化器
		initializePoseGraphOptimization();
		optimization_steps++;
		// 本机器人是否在优化顺序中。修改状态为：初始化或者空
		if (in_order)
		{
			changeOptimizerState(OptimizerState::Initialization);
		}
		else
		{
			changeOptimizerState(OptimizerState::Idle);
		}
		break;

	case OptimizerState::Initialization:
		changeOptimizerState(OptimizerState::RotationEstimation);
		rotation_estimate_start = false;
		optimization_steps++;
		break;

	case OptimizerState::RotationEstimation:
		if (rotationEstimationStoppingBarrier())
		{
			changeOptimizerState(OptimizerState::PoseEstimationInitialization);
		}
		if (current_rotation_estimate_iteration > fail_safe_steps_)
		{
			removeInactiveNeighbors();
		}
		failSafeCheck();
		optimization_steps++;
		break;

	case OptimizerState::PoseEstimationInitialization:
		initializePoseEstimation();
		pose_estimate_start = false;
		optimization_steps++;
		changeOptimizerState(OptimizerState::PoseEstimation);
		break;

	case OptimizerState::PoseEstimation:
		if (poseEstimationStoppingBarrier())
		{
			changeOptimizerState(OptimizerState::End);
		}
		failSafeCheck();
		optimization_steps++;
		break;

	case OptimizerState::End:
		endOptimization();
		changeOptimizerState(OptimizerState::PostEndingCommunicationDelay);
		optimization_steps++;
		break;

	case OptimizerState::PostEndingCommunicationDelay:
		changeOptimizerState(OptimizerState::Idle);
		optimization_steps++;
		break;
	}
}